﻿#include "ParticleSwarm.h"
#include <iostream>
#include <algorithm>
#include <functional>

using namespace std;

ParticleSwarm::ParticleSwarm()
{
	swarmSize = NUM_TOTAL_PARTICLES;
	swarmDimension = vPOS_DOF;

	gen = std::mt19937(rd());
	uniDis = std::uniform_real_distribution<>(0,1);
	uniDis_minus = std::uniform_real_distribution<>(-1,1);

	float minArray[vPOS_DOF]={
		-920,	-692,	KINECT_COLOR_NOMINAL_FOCAL_LENGTH_IN_PIXELS,
		0,		-150,	-35,	
		15,		0,		0,		-15,	
		0,		0,		0,
		-10,	0,		0,	
		0,		0,		0,	
		0,		0,		0};

	float maxArray[vPOS_DOF]={
		920,	692,	KINECT_COLOR_NOMINAL_FOCAL_LENGTH_IN_PIXELS+1000,
		135,	15,		20,	
		60,		90,		50,		80,	
		45,		90,		100,
		10,		90,		100,	
		-30,	90,		100,	
		-45,	90,		100};

	MinRange.assign(minArray,minArray+vPOS_DOF);
	MaxRange.assign(maxArray,maxArray+vPOS_DOF);

	//double randomRange[vPOS_DOF]={
	//	40,		60,		60,
	//	30,		30,		30,	
	//	30,		30,		30,		30,	
	//	30,		30,		30,
	//	30,		30,		30,	
	//	30,		30,		30,	
	//	30,		30,		30};
	
	TendencyToOwnBest = 2.8;
	TendencyToGlobalBest = 1.3;
	double gama = TendencyToOwnBest + TendencyToGlobalBest; 
	Momentum = 2/(abs(2-gama-sqrt(pow(gama,2)-4*gama))); // eq.4 - paper HandTracking - Oikonomidis Standard
	PercentMaximumVelocityOfSearchSpace = 1.0;
	UseGlobalOptimum = true;
}

void ParticleSwarm::setWristRange(double x, double y, double z, double size_x, double size_y, double size_z)
{
	MinRange[0] = x - size_x;
	MinRange[1] = y - size_y;
	MinRange[2] = z - size_z;

	MaxRange[0] = x + size_x;
	MaxRange[1] = y + size_y;
	MaxRange[2] = z + size_z;
}

ParticleSwarm::~ParticleSwarm()
{
	vector<Particle*>::iterator iter;
	for (iter = swParticles.begin(); iter != swParticles.end(); ++iter)
	{
		delete *iter;
	}
}

void ParticleSwarm::startSwarm()
{
	/* 1. Update Hypo */
	for (int i=0;i<swarmSize;i++)	swCostFunction->hypoArrays.push_back(PositionToHypo(swParticles[i]->pPosition));
	
	/* 2. Run GPU for All hypothesis */
	vector<double> measureResult(swarmSize);
	swCostFunction->psoFunction(measureResult);
	
	for (int i=0;i<swarmSize;i++)	
	{
		swParticles[i]->update_Data(measureResult[i],TRUE);		
	}

	/* 3. Update Best */
	sortParticles();
	gBestCost = current_gBestCost();
	gBestPosition = current_gBestPosition();
}

// Execute one evolution step
void ParticleSwarm::iteration()
{
	// Fitness is calculated for each particle, and update their personal best position
	/* 0. Set Particles */
	for (int i = 0; i < swarmSize; i++)
	{
		// Update new position and velocity for each particle
		if (UseGlobalOptimum)
		{
			swParticles[i]->UpdateVelocityAndPosition(gBestPosition);
		}
		else
		{
			swParticles[i]->UpdateVelocityAndPosition(current_gBestPosition());
		}
		
	}

	/* 1. Update Hypo */
	for (int i=0;i<swarmSize;i++)	swCostFunction->hypoArrays[i] = PositionToHypo(swParticles[i]->pPosition);
	
	/* 2. Run GPU for All hypothesis */
	vector<double> measureResult(swarmSize);
	swCostFunction->psoFunction(measureResult);
	
	for (int i=0;i<swarmSize;i++)	
		swParticles[i]->update_Data(measureResult[i],FALSE);
	
	/* 3. Update Best */
	sortParticles();
	if (current_gBestCost() < gBestCost)
	{
		gBestPosition = current_gBestPosition();
		gBestCost = current_gBestCost();
	}
}

// Randomize particles to avoid swarm collapse
void ParticleSwarm::randomize(double randPercent, int lowBoundDim, int highBoundDim)
{
	/* 0. Set Particles */
	int	randomSize = int(swarmSize*randPercent);
	int swarmOffset = swarmSize-randomSize;
	swCostFunction->hypoArrays.resize(randomSize);

	for (int i = swarmOffset; i < swarmSize; i++){
		int j = lowBoundDim + (int)((highBoundDim - lowBoundDim)*uniDis(gen)); //_rnd.NextDouble());
		double x = MinRange[j] + (MaxRange[j] - MinRange[j])*uniDis(gen); //_rnd.NextDouble();
		swParticles[i]->pPosition[j] = x;
		swParticles[i]->pVelocity[j] = 0; 
	
	/* 1. Update Random Hypothesis */
	//update hypo array 0 -> x
		swCostFunction->hypoArrays[i-swarmOffset] = PositionToHypo(swParticles[i]->pPosition);
	}

	/* 2. Run GPU for All hypothesis */
	vector<double> measureResult(randomSize);
	swCostFunction->psoFunction(measureResult);
	//update result 0->x to particles (max-x)->max
	for (int i=0;i<randomSize;i++)	swParticles[swarmOffset+i]->update_Data(measureResult[i],FALSE);
	swCostFunction->hypoArrays.resize(swarmSize);// set size for normal rendering

	/* 3. Update Best */
	sortParticles();
	if (current_gBestCost() < gBestCost)
	{
		gBestPosition = swParticles[0]->pPosition;
		gBestCost = swParticles[0]->pCost;
	}
}

void ParticleSwarm::endSwarm()
{
	swParticles.clear();
}

vector<double> ParticleSwarm::current_gBestPosition()
{
	return swParticles[0]->pPosition;
}


double ParticleSwarm::current_gBestCost()
{
	return swParticles[0]->pCost;
}


Particle& ParticleSwarm::operator [](int i)
{
	return *(swParticles.at(i));
}

void ParticleSwarm::sortParticles()
{
	std::sort(swParticles.begin(),swParticles.end(),CompareTo);
}

#pragma region HACK_PARTICLES

void ParticleSwarm::setRandomParticle()
{
	vector<Particle*> particles;

	for (int i=0; i < swarmSize; i++)
	{
		/// Initialize the position and velocity
		vector<double> position;
		vector<double> velocity;
		for (int j=0; j < swarmDimension; j++)
		{
			double x = MinRange[j] + (MaxRange[j] - MinRange[j])*uniDis(gen); //_rnd.NextDouble();
			double vx = 0; //MinRange[j] + (MaxRange[j] - MinRange[j])*_rnd.NextDouble();
			position.push_back(x);
			velocity.push_back(vx);
		}
		particles.push_back(new Particle(this,position,velocity));
	}
	this->swParticles = particles;
}

void ParticleSwarm::setPotentialParticle(vector<double> expectedDofs)
{
	vector<Particle*> particles;

	for (int i=0; i < swarmSize; i++)
	{
		/// Initialize the position and velocity
		vector<double> position;
		vector<double> velocity;
		// Uniform Distribution for Wrist Position
		for (int j=0; j < OFFSET_THUMB-3; j++)
		{
			//double x =  expectedDofs[j] + (30)*uniDis_minus(gen);
			double x = MinRange[j] + (MaxRange[j] - MinRange[j])*uniDis(gen);
			double vx = 0; //MinRange[j] + (MaxRange[j] - MinRange[j])*_rnd.NextDouble();
			position.push_back(x);
			velocity.push_back(vx);
		}

		//	Other Positions
		for (int j = OFFSET_THUMB-3; j < swarmDimension; j++)
		{
			double x = expectedDofs[j];
			int rdIndex1 = 3 + (int)((22 - 3)*uniDis(gen));
			int rdIndex2 = 3 + (int)((22 - 3)*uniDis(gen));
			int rdIndex3 = 3 + (int)((22 - 3)*uniDis(gen));
			//int rdIndex4 = 3 + (int)((22 - 3)*uniDis(gen));
			if (i > 8	&&	(i==rdIndex1 || i==rdIndex2 || i==rdIndex3) ){
				x = expectedDofs[j] + (30)*uniDis_minus(gen);
			}
			
			double vx = 0;
			position.push_back(x);
			velocity.push_back(vx);
		}
		particles.push_back(new Particle(this,position,velocity));
	}
	this->swParticles = particles;
}

#pragma endregion HACK_PARTICLES